#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import re
import serial
import numpy as np
import cv2
import threading
import RPi.GPIO as GPIO
import socket

global count_get_uwb#只执行一次
global x
global y
count_get_uwb=0
x=0
y=0

#服务器初始化
#-----此处可能需要更改"服务器IP"-----
host = "192.168.137.84"
#-----此处可能需要更改"端口号"-----
port = 6671               #服务器端口号
#通过更改此处来更改二号机的飞行位置
#第一位为x坐标，第二位为y坐标，第三位为模式
#1模式为起飞模式，2模式为定点飞行模式，3模式为降落模式
#-----此处可能需要更改"位置模式信息"-----
A = [2.13,',',2.14,',',1]
mySocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)    # 选择IPv4地址以及TCP协议
print ("通信服务开启")
mySocket.bind((host, port))   # 绑定端口
mySocket.listen(5)       # 监听这个端口，可连接最多5个设备
str1 = ''.join(str(i) for i in A)
mssg1 = str1.encode()
client, address=mySocket.accept()

# 连接的pixhawk飞控的端口
connection_string = '/dev/ttyACM0'
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True, baud=921600)
cap = cv2.VideoCapture(0)  # 开启摄像头
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
cap.set(cv2.CAP_PROP_FPS,30)
GPIO.setmode(GPIO.BOARD) #引脚编码方式
GPIO.setwarnings(False) #忽略GPIO警告
FORWARD_CLEAR = 12
GPIO.setup(FORWARD_CLEAR, GPIO.IN)



#定义无人机下的坐标系
def send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration=0):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control.
        0b0000111111000111,  # type_mask
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # m/s
        0, 0, 0,  # x, y, z acceleration
        0, 0)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度
def arm_and_takeoff(aTargetAltitude):
    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True
    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)
    # 发送起飞指令
    print("Taking off!")
    vehicle.simple_takeoff(1)
    time.sleep(3)
    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度
    # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print("Rangefinder distance:", vehicle.rangefinder.distance)
        if vehicle.rangefinder.distance >= aTargetAltitude * 0.95:
            print("Reached target altitude")
            time.sleep(2)
            break
        else:
            send_body_ned_velocity(0, 0, -0.1, 1)
            print("UP 0.1m")
            time.sleep(1)

#根据人脸位置返回运动逻辑
def track():
    max_w = 0
    faceRects = classifier.detectMultiScale(gray,scaleFactor=1.2,minNeighbors=3,minSize=(60, 60))
    if len(faceRects):
        for faceRect in faceRects:
            x,y,w,h = faceRect
            if w > max_w:
                max_w = w
                max_x = x
                max_y = y
                max_h = h     
        cv2.rectangle(faceImg,(max_x, max_y), (max_x + max_w, max_y + max_h), (0,255,0), 2)
        face_pos = x+w/2-width/2
        print((x+w/2)-width/2)
        if w*h > 8000:
            print("Back")
            return 2,face_pos
        if face_pos < -70 :
            print("Left")
            return -1,face_pos
        if face_pos > 70 :
            print("Right")
            return 1,face_pos
        else:
            print("Middle")
            return 0,face_pos
    return 0,0


def get_uwb():
    global count_get_uwb
    global x
    global y
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)#change tty/baud rate/timeout here
    while True:
        correct=0.5 # correction vlaue
        line = ser.readline()#read position info
        line=str(line)#byte->str
        if line:#if accepted
            a=re.search('KT',line)# find xyz information
            b=re.search('NULL',line)
            if a:
                if b:
                    print("ERROR. PLEASE CHECK UWB")
                if not b: 
                    count = line.split(',', 7) # standard
                    temp1=count[5].split('(')
                    temp2=count[7].split(')')
                    position=[0,0,0]
                    position[0]=temp1[1]
                    position[1]=count[6]
                    position[2]=temp2[0]
                    temp_x=x#上一轮次收到的位置信息
                    temp_y=y
                    x=float(position[1])#本轮收到的位置信息
                    y=float(position[0])
                    if count_get_uwb==0:
                        temp_x=x
                        temp_y=y
                        count_get_uwb=1
                    s1=x-temp_x#计算两次差值
                    s2=y-temp_y
                    if(abs(s1)>correct):#如果差值大于0.2进行修正
                        x=temp_x+np.sign(s1)*correct
                        print("Value [X] is corrected")
                    if(abs(s2)>correct):
                        y=temp_y+np.sign(s2)*correct
                        print("Value [Y] is corrected")
                    print("Camera position",x,y)
                    break
        else:
            break
    return[x,y]


def goto_pos(goal):
    print("goal is:",goal)
    while True:
        ori=get_uwb()   #当前坐标点
        print("ori",ori)
        actx=goal[0]-ori[0] #距离目标的绝对距离
        acty=goal[1]-ori[1] #距离目标的绝对距离
        if (actx>0.2):#判断前进方向
            flag=0.5#正飞
            if(actx>0.4):
                flag=1
        elif(abs(actx)<0.2):
            flag=0#       
        elif(actx<-0.2):
            flag=-0.5#反飞
            if(actx<-0.4):
                flag=-1
        if (acty>0.2):#判断前进方向
            flag2=0.5#正飞
            if(acty>0.4):
                flag2=1
        elif(abs(acty)<0.2):
            flag2=0#       
        elif(acty<-0.2):
            flag2=-0.5#反飞
            if(acty<-0.4):
                flag2=-1
        send_body_ned_velocity(flag*0.1,flag2*0.1,0,1)
        if(flag==0 and flag2==0):#距离目标点误差为0.1m以内就可以停止
            break
        time.sleep(1)
    print("ARIVIED")
    final=get_uwb()
    time.sleep(1)
    
    
#主程序
arm_and_takeoff(1)# 调用arm_and_takeoff函数，目标高度1m
mod = 0 #初始化模式为0
back=0
goal_pos=3.5 #更改此值以确定无人机降落以及发给下一个无人机起飞信号的位置
while True:
    ok, faceImg = cap.read()  # 读取摄像头图像
    if ok is False:
        print('无法读取到摄像头！')
        break
    high=faceImg.shape[0]
    width=faceImg.shape[1]
    gray = cv2.cvtColor(faceImg,cv2.COLOR_BGR2GRAY)
    #-----此处可能需要更改"文件路径"-----
    classifier = cv2.CascadeClassifier('/home/l1015/Camera/颜色识别跟踪/haarcascade_frontalface_alt.xml')
    thread = threading.Thread(target=track)
    thread.start()
    mod,fly_speed=track() #直到接受到信息为止才会执行判断
    fly_speed_dealed=fly_speed*0.001
    cam_x,cam_y=get_uwb()
    #如果到达目标位置，启动降落程序-----此处可能需要更改">"-----
    if cam_y > goal_pos:
        print("Approaching set point")
        break
    if (not GPIO.input(FORWARD_CLEAR)):
        send_body_ned_velocity(-0.3,0,0,1)
        time.sleep(0.1)
        continue;
    if (mod == 1):#向右飞
        send_body_ned_velocity(0,fly_speed_dealed,0,1)
    if (mod == -1):#向左飞
        send_body_ned_velocity(0,fly_speed_dealed,0,1)
    if (mod == 0):
        time.sleep(0.1)#
    if (mod == 2):
        send_body_ned_velocity(-0.15,0,0,1)


try:#发送起飞信息
    client.settimeout(120)                 # 设置2min时限
    client.send(mssg1)
    print("发送成功1")
    time.sleep(5)
except socket.timeout:                # 超时
    print ('time out')
#请在此设置一个安全的坐标点，让一号无人机可以在安全点降落
#-----此处可能需要更改"坐标点信息"-----
destation=[2.0,2.0]
#先去一个安全点
goto_pos(destation)
#关闭本无人机
cap.release()
# cv2.destroyAllWindows()
# 发送"降落"指令
print("Land")
# 降落，只需将无人机的飞行模式切换成"Land"
vehicle.mode = VehicleMode("LAND")
# plt.close()#注释此行可以让程序结束运行后仍然保持图像
# 退出之前，清除vehicle对象
print("Close vehicle object")
vehicle.close()
GPIO.cleanup()
mySocket.close()   #断开链接





